/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "vreptest.h"
#include "ui_vreptest.h"
#include "vrepobject.h"
#include "vrepcontrol.h"
#include <QFile>
#include <QVector>
#define MY_SIGNAL_PRO_TEST 0

VrepTest::VrepTest(QWidget *parent) :
    QWidget(parent),
    ui(new Ui::VrepTest)
{
    ui->setupUi(this);
    // axisNum=0;
    ui->doubleSpinBox_toval->setSingleStep( ui->doubleSpinBox_zj->value() );

    isRunDemo=false;

    vobj=NULL;
    vrepControl = NULL;//new VrepControl();
    connect( &timer , SIGNAL(timeout()), this , SLOT( slot_dataChanged() ) );

}

VrepTest::~VrepTest()
{
    timer.stop();
   // vrepControl->quit();
    if( vobj )
        vobj->deleteLater();
   // vrepControl->deleteLater();
    delete ui;
}

int VrepTest::startConnectVrep(int simRobotId)
{
    ui->comboBox_robot->setCurrentIndex(simRobotId);
    on_pushButton_StartVrep_toggled(true);
}

void VrepTest::displayMsg(QString msg, int code)
{
    qDebug()<<msg<<code;
    ui->textEdit_msg->append( QString("%1 , %2").arg(msg).arg(code) );
}

void VrepTest::slot_dataChanged()
{
    if( !vobj )
        return;

    static int count=0;
    count++;
    //if(count>10)
    {
        count=0;
        QString str;
        for(int i=0; i<vobj->axisNum; i++)
        {
            str.append( QString("\n%1: %2").arg( i+1 ).arg(  vobj->getPos[i] )  );
            //    qDebug()<<i<< "str=="<<vobj->getPos[i] ;
        }
        ui->label_get_value->setText( str );
        // vobj->getChanged=false;
    }
}


void VrepTest::on_doubleSpinBox_toval_valueChanged(double arg1)
{
    if( !vobj )
        return;
#if 1
    vobj->slot_set_pos( ui->comboBox_joint->currentIndex() , arg1 ) ;
#else
    emit signal_joint_change( ui->comboBox_joint->currentIndex() , arg1 ) ;
#endif

}

void VrepTest::on_doubleSpinBox_zj_valueChanged(double arg1)
{
    ui->doubleSpinBox_toval->setSingleStep( arg1 );
}

void VrepTest::on_pushButton_line_toggled(bool checked)
{
    if( !vobj )
        return;
    vobj->isRunDemo=checked;
}

void VrepTest::on_pushButton_StartVrep_toggled(bool checked)
{
    if( !vrepControl )
        return;

    if( checked )
    {
        if( !vrepControl->isRun )
        {
//            vrepControl->start(50);//这个太快会堵塞
            while(!vrepControl->isRun){
                QThread::usleep(100);
            }
        }
        qDebug()<< "====ui====QThread==========="<<QThread::currentThread();
        int ret=vrepControl->connectToVrep(ui->lineEdit_ip->text(), ui->spinBox_vrep_port->value() );
        if( ret ){
            if( !vobj )
            {
                vobj=vrepControl->vob[0];

                connect( vobj , SIGNAL(signal_msg(QString,int)), this , SLOT(displayMsg(QString,int)) ,Qt::QueuedConnection );
                connect( this , SIGNAL(signal_joint_change(int,float)), vobj , SLOT(slot_set_pos(int,float)) ,Qt::QueuedConnection );
                //connect( vobj , SIGNAL(signal_dataChanged()), this , SLOT(slot_dataChanged()) ,Qt::QueuedConnection );
                timer.start(200);
            }

            if( !vobj )
                return;

            vobj->initVrepRobot( vrepControl->clientID, ui->comboBox_robot->currentText() );
            ui->lineEdit_ip->setEnabled(!checked);
            ui->spinBox_vrep_port->setEnabled(!checked);
            ui->comboBox_joint->setCurrentIndex(0);
            #if D_USE_VREP
            ui->comboBox_io_mode->setCurrentIndex( E_ECAT_VREP );
            ui->comboBox_motor_mode->setCurrentIndex( E_ECAT_VREP );
//            vrepControl->motorSmiulationMode=E_ECAT_VREP;
//            vrepControl->ioSmiulationMode=E_ECAT_VREP;
            #endif

            slot_dataChanged();
        }else{
            displayMsg("connnect failed! ",-1);
        }

    }else{
        vrepControl->disconnectFromVrep( );
        displayMsg( "disconnect from vrep server !", 0 );

        ui->lineEdit_ip->setEnabled(!checked);
        ui->spinBox_vrep_port->setEnabled(!checked);
    }
}

void VrepTest::on_comboBox_io_mode_currentIndexChanged(int index)
{
    if( !vobj )
        return;
    vrepControl->ioSmiulationMode = ( index );
    qDebug()<<"ioSmiulationMode=="<<vrepControl->motorSmiulationMode ;
}

void VrepTest::on_comboBox_motor_mode_currentIndexChanged(int index)
{
    if( !vobj )
        return;
    vrepControl->motorSmiulationMode = ( index );
    qDebug()<<"motorSmiulationMode=="<<vrepControl->motorSmiulationMode ;
}


void VrepTest::on_comboBox_robot_currentIndexChanged(const QString &arg1)
{
    if( !vobj )
        return;
    vobj->initVrepRobot( vrepControl->clientID, arg1);
    ui->comboBox_joint->clear();
    for(int i=0; i<vobj->axisNum; i++)
    {
        ui->comboBox_joint->addItem( QString("关节: %1").arg( 1+i ) );
    }
}


//void VrepTest::on_pushButton_jian_clicked()
//{
//    if( vobj ){
//        ui->doubleSpinBox_toval->setValue( vobj->getPos[ ui->comboBox_joint->currentIndex() ]  - ui->doubleSpinBox_zj->value() );
//    }
//}

//void VrepTest::on_pushButton_jia_clicked()
//{
//    {
//        ui->doubleSpinBox_toval->setValue( vobj->getPos[ ui->comboBox_joint->currentIndex() ]  + ui->doubleSpinBox_zj->value() );
//    }
//}

void VrepTest::on_pushButton_jian_pressed()
{
    if( vobj ){
        ui->doubleSpinBox_toval->setValue( ui->doubleSpinBox_toval->value() - ui->doubleSpinBox_zj->value() );
    }
}

void VrepTest::on_pushButton_jia_pressed()
{
    if( vobj ){
        ui->doubleSpinBox_toval->setValue(  ui->doubleSpinBox_toval->value()  + ui->doubleSpinBox_zj->value() );
    }
}

void VrepTest::on_comboBox_joint_currentIndexChanged( int index )
{
    if( vobj ){
        ui->doubleSpinBox_toval->setValue( vobj->getPos[ index ] );
    }
}


